#include "../include/DataIO.hpp"

// using namespace std;
DataIO::DataIO()
{
}
void DataIO::Read_image(string image_dir, string image_time_dir, vector<string> &img_names, vector<double> &timestamps)
{
    ifstream fTimes;
    fTimes.open(image_time_dir.c_str());
    img_names.reserve(5000);
    timestamps.reserve(5000);
    while (!fTimes.eof())
    {
        string s;
        getline(fTimes, s);
        if (s[0] == '#')
        {
            continue;
        }
        string item;
        size_t pos = 0;
        double time_stamp;
        if (!s.empty())
        {
            while ((pos = s.find(',')) != string::npos)
            {
                item = s.substr(0, pos);
                time_stamp = stod(item);
                s.erase(0, pos + 1);
            }
            item = s.substr(0, pos);

            timestamps.push_back(time_stamp / 1e9);
            img_names.push_back(image_dir + "/" + item);
        }
        // if (!s.empty())
        // {
        //     stringstream ss;
        //     ss << s;
        //     img_names.push_back(image_dir + "/" + ss.str() + ".png");
        //     double t;
        //     ss >> t;
        //     timestamps.push_back(t / 1e9);
        // }
    }
}

void DataIO::Read_imu(string imudata_dir, vector<double> &timestamps, vector<Point3f> &v_acc, vector<Point3f> &v_gyro)
{
    ifstream fimu;
    fimu.open(imudata_dir.c_str());
    timestamps.reserve(5000);
    v_acc.reserve(5000);
    v_gyro.reserve(5000);
    while (!fimu.eof())
    {
        string s;
        getline(fimu, s);
        if (s[0] == '#')
        {
            continue;
        }
        if (!s.empty())
        {
            string item;
            int pos = 0;
            double data[7];
            int count = 0;
            while ((pos = s.find(",")) != string::npos)
            {
                item = s.substr(0, pos);
                data[count++] = stod(item);
                s.erase(0, pos + 1);
            }

            item = s.substr(0, pos);
            data[6] = stod(item);

            timestamps.push_back(data[0] / 1e9);
            v_acc.push_back(Point3f(data[4], data[5], data[6]));
            v_gyro.push_back(Point3f(data[1], data[2], data[3]));
        }
    }
}
// void DataIO::Set_image_dir(string dir)
// {
//     ms_image_dir = dir;
// }
// void DataIO::Set_image_timestamp_dir(string dir)
// {
//     ms_image_timestamp_dir = dir;
// }

// void DataIO::Set_imudata_dir(string dir)
// {
//     ms_imudata_dir = dir;
// }